﻿/*
 *  Copyright (C) 2019  刘臣轩
                  2021  贺龙宇
                  2022  贺龙宇 张伊男
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program. If not, see <http://www.gnu.org/licenses/>.
 */
#include "ArmorDetector.h"
ArmorDetector::ArmorDetector()
{
    //number_template.emplace_back(cv::Mat());
    // for (int i = 0; i <= 8; ++i)
    // {
    //     number_template.emplace_back(cv::imread(std::to_string(i) + ".png", cv::IMREAD_GRAYSCALE));
    //     cv::resize(number_template[i],number_template[i],cv::Size(100,100));
    //     cv::threshold(number_template[i],number_template[i],100,255,cv::THRESH_BINARY);
    //     // cv::imshow(std::to_string(i),number_template[i]);
    // }
    //1.kalman filter setup
    // KF.init(state_num, measure_num, 0);
    // KF.transitionMatrix = (cv::Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);  //转移矩阵A
    // setIdentity(KF.measurementMatrix);                                             //测量矩阵H
    // setIdentity(KF.processNoiseCov, cv::Scalar::all(1e-5));                            //系统噪声方差矩阵Q
    // setIdentity(KF.measurementNoiseCov, cv::Scalar::all(1e-1));                        //测量噪声方差矩阵R
    // setIdentity(KF.errorCovPost, cv::Scalar::all(1));                                  //后验错误估计协方差矩阵P
    // rng.fill(KF.statePost, cv::RNG::UNIFORM, 0, 480);   //初始状态值x(0)
    // measurement = cv::Mat::zeros(measure_num, 1, CV_32F);                           //初始测量值，因为后面要更新这个值，所以必须先定义
    // svm = cv::ml::SVM::create();
    // svm = cv::Algorithm::load<cv::ml::SVM>("svm.xml");

    // cv::Mat sample,response;

    // cv::FileStorage Data("TrainingData.yml", cv::FileStorage::READ);
    // Data["data"] >> sample;
    // Data.release();

    // cv::FileStorage Label("LabelData.yml", cv::FileStorage::READ);
    // Label["label"] >> response;
    // Label.release();
    
    // knn->cv::Ptr<cv::ml::KNearest>(cv::ml::KNearest::create());
    knn=cv::ml::KNearest::create();
    knn=cv::ml::StatModel::load<cv::ml::KNearest>("knn_num.yml");
}

ArmorDetector::~ArmorDetector()
{
}

bool ArmorDetector::DetectArmor(cv::Mat& src, std::vector<cv::Rect> &box, std::vector<int> &id)
{
    // int start_time=std::clock();
#ifdef CALC_TIME

    // static int times = 0;
    // static double sum = 0;
    // double t = (double)cv::getTickCount();//计算程序耗时

#endif

#ifdef DEBUG

        show_lights = src.clone();
        show_armors = src.clone();
        show_final_armor = src.clone();
        show_rect=src.clone();
        //printf("%d\n", mcu_data.enemy_color);
        // cv::imshow("src", src);
        


#endif
    // SerialChecking();
    GetPrimaryThreshold(src);
    std::vector<cv::RotatedRect> lights;
    DetectLights(src, lights);/*灯条判定*/

    std::vector<std::vector<cv::Point2f>> armors;
    PossibleArmors(lights, armors);/*装甲板判定*/
    if (armors.empty()) {
        return false;
    }


    GetArmorNumber(src, armors, id);/*装甲板选择*/
    

    for(auto i=0;i<armors.size();i++){
        cv::Rect rect;
        rect.x=armors[i][0].x;
        rect.y=armors[i][0].y;
        rect.width=(armors[i][1].x-armors[i][0].x+armors[i][2].x-armors[i][3].x)/2.0;
        rect.height=(armors[i][3].y-armors[i][0].y+armors[i][2].y-armors[i][1].y)/2.0;
        box.emplace_back(rect);
#ifdef DEBUG
        
        cv::rectangle(show_rect,rect,cv::Scalar(255,255,255),2);
        cv::putText(show_rect,std::to_string(id[i]),cv::Point(rect.x+rect.width/2.0,rect.y+rect.height/2.0),0,1,cv::Scalar(0,255,0),4);
#endif
        // std::cout<<box.size();
    }
#ifdef DEBUG
    cv::imshow("rect",show_rect);
#endif
    // CalcControlInfo(final_armor, target);/*计算*/
    //Kalman_Predict(final_armor, target);//预测

// int end_time=std::clock();
// std::cout<<(double)(end_time-start_time)/CLOCKS_PER_SEC<<std::endl;
#ifdef CALC_TIME

    // t = (double)cv::getTickCount() - t;
    // sum += t * 1000. / cv::getTickFrequency();
    // times++;
    // if (true)
    // {
    //     printf("execution time = %gms\n", sum / times);//转换时间单位并输出代码运行时间
    //     times = 0;
    //     sum = 0;
    // }

#endif
    return true;
}

cv::Mat ArmorDetector::DistillationColor(const cv::Mat& src)/*颜色二值化*/
{
    std::vector<cv::Mat> bgr;
    cv::split(src, bgr);
    cv::Mat result;
    if (mcu_data.color == EnemyColor::BLUE)
        cv::subtract(bgr[0], bgr[2], result);
    else
        cv::subtract(bgr[2], bgr[0], result);
    return result;
}

inline void ArmorDetector::DrawRotatedRect(const cv::Mat& image, const cv::RotatedRect& rect, const cv::Scalar& color, int thickness)/*标记矩形函数*/
{
    cv::Point2f vertices[4];
    rect.points(vertices);
    for (int i = 0; i < 4; i++)
        cv::line(image, vertices[i], vertices[(i + 1) % 4], color, thickness, cv::LINE_AA);
}

void ArmorDetector::DetectLights(const cv::Mat& src, std::vector<cv::RotatedRect>& lights)
{
    cv::Mat gray_image;
    cv::cvtColor(src, gray_image, cv::COLOR_BGR2GRAY);
    cv::Mat binary_brightness_image;
    float thresh_brightness = (mcu_data.color == EnemyColor::BLUE) ? armorParam.brightness_thresh_blue : armorParam.brightness_thresh_red;
    cv::threshold(gray_image, binary_brightness_image, thresh_brightness, 255, cv::THRESH_BINARY);

    cv::Mat binary_color_image;
    cv::Mat distillation = DistillationColor(src);

    float thresh_color = (mcu_data.color == EnemyColor::BLUE) ? armorParam.blue_thresh : armorParam.red_thresh;
    cv::threshold(distillation, binary_color_image, thresh_color, 255, cv::THRESH_BINARY);
    // cv::morphologyEx(binary_color_image, binary_color_image, cv::MORPH_CLOSE, cv::Mat());

    // if(mcu_data.color==EnemyColor::RED)
        cv::dilate(binary_color_image, binary_color_image, cv::Mat());  /*膨胀*/
    // else
    //     {
    //         cv::dilate(binary_color_image, binary_color_image, cv::Mat(),cv::Point(-1,-1),2);  /*膨胀*/
    //         cv::erode(binary_color_image, binary_color_image, cv::Mat(),cv::Point(-1,-1),1);
    //     }

    std::vector<std::vector<cv::Point>> contours_color; /*颜色轮廓*/
    cv::findContours(binary_color_image, contours_color, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
    std::vector<std::vector<cv::Point>> contours_brightness; /*亮度轮廓*/
    cv::findContours(binary_brightness_image, contours_brightness, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);

    lights.reserve(contours_color.size());

    /*若识别蓝色，则方法为判断颜色中心是否在亮度轮廓内*/
    if(mcu_data.color==EnemyColor::BLUE)
        for (unsigned int i = 0; i < contours_color.size(); ++i) {
            cv::RotatedRect single_light=cv::minAreaRect(contours_color[i]);

            for (unsigned int j = 0; j < contours_brightness.size(); ++j) {
                if (cv::pointPolygonTest(contours_brightness[j], single_light.center, false) >= 0.0) {     /*判断颜色轮廓中心是否在亮度轮廓内*/
                    // cv::RotatedRect single_light = cv::minAreaRect(contours_color[j]);
                    cv::Rect bounding_rect = single_light.boundingRect();/*得到一个垂直的矩形*/
                    if (bounding_rect.height < bounding_rect.width || single_light.size.area() < armorParam.light_min_area)    /*垂直矩形的高度大于宽度，且面积大于最小值*/
                        break;
                
                    /*判断灯条长宽比例*/
                    float height=std::max(single_light.size.height,single_light.size.width);
                    float width=std::min(single_light.size.height,single_light.size.width);
                    float ratio=height/width;
                    if(ratio>armorParam.light_max_ratio) break;

                    /*判断灯条旋转角度*/
                    float rotated_angle=(single_light.size.width>single_light.size.height?90-single_light.angle:single_light.angle);
                    if(rotated_angle>armorParam.light_max_rotated_angle) break;
                    // std::cout<<ratio<<std::endl;


                    lights.emplace_back(single_light);
                    break;
                }
            }
            // std::cout<<std::endl;
        }

        /*若识别红色，则方法为判断亮度中心是否在颜色轮廓内*/
        else{
            for (unsigned int i = 0; i < contours_brightness.size(); ++i) {

            cv::RotatedRect brightness_rect=cv::minAreaRect(contours_brightness[i]);

            for (unsigned int j = 0; j < contours_color.size(); ++j) {
                if (cv::pointPolygonTest(contours_color[j], brightness_rect.center, false) >= 0.0) {     /*判断亮度点是否在颜色轮廓内*/
                    cv::RotatedRect single_light = cv::minAreaRect(contours_color[j]);
                    cv::Rect bounding_rect = single_light.boundingRect();/*得到一个垂直的矩形*/
                    if (bounding_rect.height < bounding_rect.width || single_light.size.area() < armorParam.light_min_area)    /*垂直矩形的高度大于宽度，且面积大于最小值*/
                        break;
                
                    /*判断灯条长宽比例*/
                    float height=std::max(single_light.size.height,single_light.size.width);
                    float width=std::min(single_light.size.height,single_light.size.width);
                    float ratio=height/width;
                    if(ratio>armorParam.light_max_ratio) break;
                    // std::cout<<ratio<<std::endl;

                    /*判断灯条旋转角度*/
                    float rotated_angle=(single_light.size.width>single_light.size.height?90-single_light.angle:single_light.angle);
                    if(rotated_angle>armorParam.light_max_rotated_angle) break;

                    lights.emplace_back(single_light);
                    break;
                }
            }
        // std::cout<<std::endl;
        }
    }

#ifdef DEBUG


        for (auto light : lights)
            DrawRotatedRect(show_lights, light, cv::Scalar(0, 255, 0), 2);
        // cv::imshow("distillation", distillation);
        cv::imshow("binary_brightness_image", binary_brightness_image);
        cv::imshow("binary_color_img", binary_color_image);
        cv::imshow("show_lights", show_lights);


#endif

}

inline void ArmorDetector::DrawArmor(const cv::Mat& image, const std::vector<cv::Point2f>& armor, const cv::Scalar& color, int thickness)/*标注装甲板*/
{
    for (int i = 0; i < 4; i++)
        cv::line(image, armor[i], armor[(i + 1) % 4], color, thickness, cv::LINE_AA);
}

void ArmorDetector::PossibleArmors(const std::vector<cv::RotatedRect>& lights, std::vector<std::vector<cv::Point2f>>& armors)
{
    for (unsigned int i = 0; i < lights.size(); ++i) {
        for (unsigned int j = i + 1; j < lights.size(); ++j) {
            float angle_i, angle_j;

            if(lights[i].size.width<lights[i].size.height) {
                angle_i=lights[i].angle;
            }
            else{
                angle_i=90-lights[i].angle;
            }

            if(lights[j].size.width<lights[j].size.height) {
                angle_j=lights[j].angle;
            }
            else{
                angle_j=90-lights[j].angle;
            }

            float angle_diff = std::abs(angle_i - angle_j);                   /*由于opencv的角度选择随矩形旋转角度不同并不一致，为方便处理这里统一进行变换*/
             //std::cout << "\nangle_diff = " << angle_diff;
            if (angle_diff > armorParam.armor_max_angle_diff)
                continue;

            double armor_angle = (double)std::atan((lights[i].center.y - lights[j].center.y) / (lights[i].center.x - lights[j].center.x)) * 180 / CV_PI;
            // std::cout << "\nangle_i = " << angle_i;
            // std::cout << "\nangle_j = " << angle_j;
            // std::cout << "\narmor_angle = " << armor_angle;
            if (std::max(abs(armor_angle - angle_i), abs(armor_angle - angle_j)) > armorParam.armor_max_angle_diff
		&& std::max(abs(armor_angle - angle_i), abs(armor_angle - angle_j)) < 90-armorParam.armor_max_angle_diff) /*角度过大*/
	    {
		    //printf("angle diff too large \n");
		    continue;
	    }

            float height_i = std::max(lights[i].size.width, lights[i].size.height);
            float height_j = std::max(lights[j].size.width, lights[j].size.height);
            auto height = std::minmax(height_i, height_j);
            //std::cout << "\nheight_i/height_j = " << height.second / height.first << '\n';
            if ((height.second / height.first) > armorParam.armor_max_height_ratio)  /*两个灯条的高度差*/
                continue;
            
            /*两个灯条位置差*/
            /*若两灯条的y坐标的差值比最长的灯条还大，则认为不是装甲板*/
            float center_y_high=std::max(lights[i].center.y,lights[j].center.y);
            float center_y_low=std::min(lights[i].center.y,lights[j].center.y);
            float max_light_height=std::max(height_i,height_j);
            
            if(center_y_high-center_y_low>max_light_height)
                continue;
            

            float armor_width = std::sqrt((lights[i].center.x - lights[j].center.x) * (lights[i].center.x - lights[j].center.x) + (lights[i].center.y - lights[j].center.y) * (lights[i].center.y - lights[j].center.y));
            float armor_height = (height_i + height_j) / 2;
            // std::cout << "\narmor_width = " << narmor_width;
            // std::cout << "\narmor_height = " << armor_height;
           // std::cout << "\nwidth/height = " << armor_width / armor_height;
            if ((armor_width / armor_height) > armorParam.armor_max_aspect_ratio)  /*装甲板的宽高比*/
                continue;

            std::vector<cv::Point2f> armor;
            if (lights[i].center.x < lights[j].center.x)
                CalcArmorInfo(armor, lights[i], lights[j]);
            else
                CalcArmorInfo(armor, lights[j], lights[i]);  
            armors.emplace_back(armor);
        }
    }

#ifdef DEBUG

        for (auto armor : armors)
            DrawArmor(show_armors, armor, cv::Scalar(0, 255, 0), 2);
        cv::imshow("show_armors", show_armors);


#endif

}

void ArmorDetector::CalcArmorInfo(std::vector<cv::Point2f>& armor, const cv::RotatedRect& left_light, const cv::RotatedRect& right_light) 
/*计算装甲板的角点*/
{
    cv::Point2f left_points[4], right_points[4];
    left_light.points(left_points);
    right_light.points(right_points);

    std::sort(left_points, left_points + 4, [](const cv::Point2f& p1, const cv::Point2f& p2) { return p1.y < p2.y; });
    cv::Point2f left_up = (left_points[0].x > left_points[1].x) ? left_points[0] : left_points[1];
    cv::Point2f left_down = (left_points[2].x > left_points[3].x) ? left_points[2] : left_points[3];
    cv::Point2f left_shift = (left_down - left_up) * armorParam.shift_scale;

    std::sort(right_points, right_points + 4, [](const cv::Point2f& p1, const cv::Point2f& p2) { return p1.y < p2.y; });
    cv::Point2f right_up = (right_points[0].x < right_points[1].x) ? right_points[0] : right_points[1];
    cv::Point2f right_down = (right_points[2].x < right_points[3].x) ? right_points[2] : right_points[3];
    cv::Point2f right_shift = (right_down - right_up) * armorParam.shift_scale;

    armor.emplace_back(left_up - left_shift);
    armor.emplace_back(right_up - right_shift);
    armor.emplace_back(right_down + right_shift);
    armor.emplace_back(left_down + left_shift);
}

int ArmorDetector::DetectNumber(const cv::Mat& perspective, const ArmorType &armor_type) /*判断数字，方法为SVM*/
{
    ///*使用svm进行，上赛季效果不好，已弃用*/
    // cv::Mat tmp, tmp1;
    // cv::resize(perspective, tmp, cv::Size(10, 10));
    // tmp.convertTo(tmp1, CV_32FC1);
    // float result = svm->predict(tmp1.reshape(1, 1));
    // return (int)result;


    ///*使用模板匹配进行数字识别*/
    //需要进一步优化识别检测逻辑，误识别情况还是比较明显（目前已优化）
    // cv::Mat tmp;
    // cv::resize(perspective,tmp,cv::Size(100,100));
    // // cv::erode(tmp,tmp,cv::Mat(),cv::Point(-1,-1),4);
    // // cv::dilate(tmp,tmp,cv::Mat(),cv::Point(-1,-1),4);
    // int cnt=0,max_cnt=-99999,max_cnt_index=0;
    // double maxm=-9999;
    // cv::Mat result;
    // for(int i=1;i<=8;i++){

    //     if(armor_type==ArmorType::BIG && i>=2 && i<=6) continue;
    //     if(armor_type==ArmorType::SMALL && (i==1 || i==7 || i==8) ) continue;
        
    //     cv::matchTemplate(tmp,number_template[i],result,cv::TM_CCOEFF_NORMED);
    //     double minval,maxval;
    //     cv::Point minloc,maxloc;
    //     cv::minMaxLoc(result,&minval,&maxval,&minloc,&maxloc);
    //     if(maxval>maxm){
    //         maxm=maxval;
    //         max_cnt_index=i;
    //     }
    //     // std::cout<<i<<" "<<maxval<<std::endl;
    // }
    // // std::cout<<"\n\n";

    // if(maxm<0.2) return 0;/*参数可调*/
    // return max_cnt_index;

    /*使用KNN进行数字识别*/
    cv::Mat tmp;
    cv::resize(perspective, tmp, cv::Size(70,70), 0, 0, cv::INTER_LINEAR);
    tmp.convertTo(tmp, CV_32FC1);
    std::vector<float> dis;

    cv::Mat result, neighborResponses, dists;
    float p = knn->findNearest(tmp.reshape(1, 1), 1, result, neighborResponses, dis);
    // std::cout<<dis[0]<<std::endl;
    
    static int cnt=0;
    if(cv::mean(dis)[0]>armorParam.knn_number_min_belief) 
    {
        // cv::imwrite(std::to_string(++cnt)+".png",perspective);
        return 0;
    }

    /*由于基地和前哨站很像，因此需要根据大小装甲板来判断*/
    // if((int)p==6 || (int)p==8){
    //     if(armor_type==ArmorType::SMALL)
    //         return 6;
    //     else 
    //         return 8;
    // }
    // std::cout<<(int)p<<" "<<cv::mean(dis)[0]<<std::endl;
    return (int)p;
}

ArmorType ArmorDetector::DetectArmorType(std::vector<cv::Point2f> &armor){
    float armor_width=(armor[1].x-armor[0].x+armor[2].x-armor[3].x)*0.5;
    float armor_height=(armor[2].y-armor[1].y+armor[3].y-armor[0].y)*0.5;

    if(armor_width/armor_height>0.8){ /*这个数待测*/
        return ArmorType::BIG;
    }

    return ArmorType::SMALL;
    
}

void ArmorDetector::GetArmorNumber(const cv::Mat& src, std::vector<std::vector<cv::Point2f>>& armors, std::vector<int>& id)
{
    //判断标签
    cv::Mat gray;
    cv::cvtColor(src, gray, cv::COLOR_BGR2GRAY);
    
    // cv::imshow("gray",gray);

    static float half_camera_width = cameraParam.resolution_width / 2;
    static float half_camera_height = cameraParam.resolution_height / 2;
    std::vector<float> armors_distance(armors.size()); //armors_distance 距离中心的距离
    for (int i = 0; i < armors.size(); i++) {
        if(!GetNumberThreshold(src,armors[i])){
            id.emplace_back(0);
            continue;
        }
        cv::Mat dst;
        cv::threshold(gray, dst, armorParam.number_thresh, 255, cv::THRESH_BINARY);
        // cv::imshow("gray",dst);
        cv::Mat perspective;
        cv::Mat homography=cv::getPerspectiveTransform(armors[i],armor_points);
        cv::warpPerspective(dst, perspective, homography, cv::Size(armorParam.small_armor_width, armorParam.armor_height));/*该处为矩阵变换*/
#ifdef DEBUG
	if(i<=4){
        	cv::imshow("p"+std::to_string(i),perspective);
	}
#endif
        int n = DetectNumber(perspective,DetectArmorType(armors[i]));
        
        /*保存perspective*/
        // static int count=10;
        // static int cnt=10;
        // cnt++;
        // if(cnt>=10){
        //     cv::imwrite(std::to_string(++count)+".png",perspective);
        //     cnt=0;
        // }


        id.emplace_back(n);
        
    }
}

void ArmorDetector::GetPrimaryThreshold(const cv::Mat &src){
    
    cv::Mat gray_image;
    cv::cvtColor(src,gray_image,cv::COLOR_BGR2GRAY);

    // cv::Mat channels[3];
    // cv::split(hsv_image,channels);

    

    double avg_light=cv::mean(gray_image)[0];
    // std::cout<<avg_light<<std::endl;

    // avg_light=(avg_light/src.rows/src.cols);
    if(avg_light>100) 
        avg_light=100;

    // armorParam.brightness_thresh_red=-1e-5*pow(avg_light,4)+0.0023*pow(avg_light,3)-0.1426*pow(avg_light,2)+3.5936*avg_light+10.417;
    // armorParam.brightness_thresh_blue=0.0068*avg_light*avg_light+1.0085*avg_light+65.333;

    /*调整阈值*/

    if(avg_light<10){
        armorParam.brightness_thresh_red=50+armorParam.brightness_red_correct;
        armorParam.brightness_thresh_blue=160+armorParam.brightness_blue_correct;
        
        armorParam.red_thresh=70+armorParam.color_red_correct;
        armorParam.blue_thresh=145+armorParam.color_blue_correct;
    }
    else if(avg_light<20){
        armorParam.brightness_thresh_red=75+armorParam.brightness_red_correct;
        armorParam.brightness_thresh_blue=160+armorParam.brightness_blue_correct;

        armorParam.red_thresh=70+armorParam.color_red_correct;
        armorParam.blue_thresh=145+armorParam.color_blue_correct;
    }
    else if(avg_light<30){
        armorParam.brightness_thresh_red=85+armorParam.brightness_red_correct;
        armorParam.brightness_thresh_blue=160+armorParam.brightness_blue_correct;

        armorParam.red_thresh=70+armorParam.color_red_correct;
        armorParam.blue_thresh=145+armorParam.color_blue_correct;
    }
    else if(avg_light<40){
        armorParam.brightness_thresh_red=105+armorParam.brightness_red_correct;
        armorParam.brightness_thresh_blue=160+armorParam.brightness_blue_correct;

        armorParam.red_thresh=70+armorParam.color_red_correct;
        armorParam.blue_thresh=115+armorParam.color_blue_correct;
    }
    else if(avg_light<50){
        armorParam.brightness_thresh_red=105+armorParam.brightness_red_correct;
        armorParam.brightness_thresh_blue=190+armorParam.brightness_blue_correct;

        armorParam.red_thresh=70+armorParam.color_red_correct;
        armorParam.blue_thresh=115+armorParam.color_blue_correct;
    }
    else{
        armorParam.brightness_thresh_red=135+armorParam.brightness_red_correct;
        armorParam.brightness_thresh_blue=190+armorParam.brightness_blue_correct;

        armorParam.red_thresh=90+armorParam.color_red_correct;
        armorParam.blue_thresh=115+armorParam.color_blue_correct;
    }



    /*调整颜色阈值*/
   
    // std::cout<<avg_light<<std::endl;
}

bool ArmorDetector::GetNumberThreshold(const cv::Mat &src, const std::vector<cv::Point2f> &armor){
    // cv::Rect rect(armor[0].x,armor[0].y,armor[1].x-armor[0].x,armor[2].y-armor[1].y);
    

    if(armor[0].x>armor[1].x || armor[0].x>armor[2].x || armor[3].x>armor[1].x || armor[3].x>armor[2].x
    || armor[0].y>armor[3].y || armor[0].y>armor[2].y || armor[1].y>armor[3].y || armor[1].y>armor[2].y){
        return false;
    }

    cv::Mat roi=src(cv::Range(std::max((float)(1.0),armor[0].y),std::min((float)(src.rows),armor[3].y)),
                    cv::Range(std::max((float)1.0,armor[0].x),std::min((float)src.cols,armor[1].x)));

    if(roi.empty()){
        return false;
    }
    // cv::imshow("roi",roi);
    // cv::Mat channels[3];
    cv::Mat gray_roi;
    cv::cvtColor(roi,gray_roi,cv::COLOR_BGR2GRAY);
    // cv::split(hi,channels);
    double avg_light=cv::mean(gray_roi)[0];
    
    /*4.23 更新阈值计算方案*/

    if(mcu_data.color==EnemyColor::RED){
        armorParam.number_thresh=avg_light+armorParam.number_red_correct;
    }

    /*EnemyColor==BLUE*/
    else{
        /*4.23 测试数据如下
        *平均亮度：80 合适阈值：120*
        *平均亮度：70 合适阈值：90*
        *平均亮度：60 合适阈值：60*
        *平均亮度：50 合适阈值：40*
        *平均亮度：40 合适阈值：25*
        *平均亮度：30 合适阈值：15*
        *平均亮度：20 合适阈值：8*
        
        这个阈值不太好处理，以下为一种处理方案，并不是最优方案
        多项式拟合试过了，会出问题
        
        */

        // if(avg_light>60){
        //     armorParam.number_thresh=75;
        // }
        // else if(avg_light>40){
        //     armorParam.number_thresh=20;
        // }
        // else if(avg_light>20){
        //     armorParam.number_thresh=8;
        // }
        // else {
        //     armorParam.number_thresh=5;
        // }

        // armorParam.number_thresh=0.0268*avg_light*avg_light-0.8179*avg_light+armorParam.number_blue_correct;
        
        armorParam.number_thresh=avg_light+armorParam.number_red_correct;
    }

    // if(avg_light>100) avg_light=100;
    // if(avg_light<5)
    // armorParam.number_thresh=avg_light+1;
    // else
    // if(mcu_data.color==EnemyColor::BLUE) armorParam.number_thresh=1.5*avg_light+5;
    // else armorParam.number_thresh=0.65*avg_light;
    // std::cout<<avg_light<<std::endl;
    return true;
}

void ArmorDetector::SerialChecking(void){
/****************************************************
* 基本逻辑：若接收到的串口数据与上一次的不同，说明接收正常
*          将cnt重置
          若接收数据一致，令cnt++
          若cnt累计到一定数量时，认为串口接收异常，设置状态

* 在异常状态下，若接收数据发生改变，则认为串口恢复
* 此时设置状态并重置cnt
****************************************************/
    static int cnt=0;
    static McuData last_mcu_data={0,EnemyColor::RED,State::ARMOR_STATE,0,0,0};
    if(mcu_data.gimbal_pitch==last_mcu_data.gimbal_pitch
        && mcu_data.gimbal_yaw==last_mcu_data.gimbal_yaw){
            cnt++;
    }
    else cnt=0;

    if(serial_states==SERIAL_NORMAL && cnt>=50){
        FYTLog->writeLog("Serial changed to abnormal mode");
        serial_states=SERIAL_ABNOMAL;
    }
    else if(serial_states==SERIAL_ABNOMAL && cnt==0){
        FYTLog->writeLog("Serial changed to normal mode");
        serial_states=SERIAL_NORMAL;
    }
    last_mcu_data.gimbal_yaw=mcu_data.gimbal_yaw;
    last_mcu_data.gimbal_pitch=mcu_data.gimbal_pitch;
}
